/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file dataToXML.cxx
  \brief class implemenation XML generator

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <iostream>
#include <stdio.h>
#include <math.h>

#include "apSim.h"
#include "utilities.h"

/*<-------Defines--------->*/
#define POSITION_UPDATE_PERIOD 0.2
#define TELEMETRY_PERIOD 1.0
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
uint8_t ap_mode = AP_PRELAUNCH;
uint8_t ob_mode = OB_WAYPT;
/*<-End Global Variables-->*/


/************************************************************\
|                  DataToXML_Parser::init                    |
\************************************************************/
/*!
  \brief the initializing function for the XML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToXML_Parser has been created
  \post the XML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
*/

APSimThread::APSimThread(String name, float v_min, float v_max, float dpsi_max) : ThreadedPipe(name)
{
	srand(time(0));
	waypoints[0].number   = 0;
	waypoints[0].next     = 0;
	waypoints[0].orbitRad = 100;
	//waypoints[0].pos.lat  = 40.14  + (rand() / (float)RAND_MAX - 0.5) * 0.02;
	//waypoints[0].pos.lon  = -105.25 + (rand() / (float)RAND_MAX - 0.5) * 0.02;
	waypoints[0].pos.lat  = 0;
	waypoints[0].pos.lon  = 0;
	waypoints[0].pos.alt  = 0;

	current_waypoint                = 0;
	telemetry.gps.utc.hours         = 0;
	telemetry.gps.utc.minutes       = 0;
	telemetry.gps.utc.seconds       = 0.0;
	telemetry.gps.pos.lat           = waypoints[0].pos.lat;
	telemetry.gps.pos.lon           = waypoints[0].pos.lon;
	telemetry.gps.pos.alt           = 0;
	telemetry.gps.track             = 0;
	telemetry.gps.sog               = (v_max+v_min)/2;
	telemetry.gps.accuracy.quality  = 0;
	telemetry.gps.accuracy.sats     = 0;
	telemetry.gps.accuracy.pdop     = 0.0;

	limits.v_max = v_max;
	limits.v_min = v_min;
	limits.dpsi_max = (dpsi_max*M_PI/180);

	ap_rates.turn_rate = 0;
	ap_rates.velocity = 0;
}

bool APSimThread::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	last_telem_tx.stamp();
	last_position_update.stamp();
	return true;
}

void APSimThread::update()
{
	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	struct timeval timeout;

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {
		/*<---RX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			if((n = readFifo(INBOARD_RX)) > 0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_FP: {
								ap_mode = AP_FLYING;
								ob_mode = OB_WAYPT;
								int num_waypts = cmd.size()/sizeof(Waypt_t);
								if(num_waypts > NUM_WAYPOINTS)
									num_waypts = NUM_WAYPOINTS;

								memcpy(&waypoints,dataPtr,num_waypts*sizeof(Waypt_t));
								current_waypoint = 0;
								// if node is way to far away, teleport it
								if(GPSRange(telemetry.gps.pos, waypoints[current_waypoint].pos) > 20000) { 
									telemetry.gps.pos.lat = waypoints[current_waypoint].pos.lat;
									telemetry.gps.pos.lon = waypoints[current_waypoint].pos.lon;
									telemetry.gps.pos.alt = waypoints[current_waypoint].pos.alt;
								}

								ACK(INBOARD_TX);

							} break;
						case CMD_AP_RATES: {
								ap_mode = AP_FLYING;
								ob_mode = OB_TURNRATE;
								memcpy(&ap_rates,dataPtr,sizeof(APRates_t));
                //cout << getName() << "::parse - velocity (" << ap_rates.velocity << ") and turn rate (" << ap_rates.turn_rate << ") received" <<endl;

							} break;
					}
				}
			}
		}
		/*<---TX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_telem_tx ) > (TELEMETRY_PERIOD * SEC2MICRO) ) {
				Command cmd;
				char * dataPtr = (char *)CMD_DATAPTR(fifo_msg.data);

				CMD_CMD((fifo_msg.data)) = CMD_TELEM;
				CMD_NUM((fifo_msg.data)) = sizeof(Telemetry_t);
				CopyStructToBuf(telemetry, dataPtr);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				Client * a_client;
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_TELEM,i)) != NULL)
				{
					Client * me = shmPtr->clients.getClient(0);
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}

				i=0;

				GPSTelem_t gps_telem;

				gps_telem.utc.hours = telemetry.gps.utc.hours;
				gps_telem.utc.minutes = telemetry.gps.utc.minutes;
				gps_telem.utc.seconds = telemetry.gps.utc.seconds;
				gps_telem.pos.lat = telemetry.gps.pos.lat;
				gps_telem.pos.lon = telemetry.gps.pos.lon;
				gps_telem.pos.alt = telemetry.gps.pos.alt;
				gps_telem.track = telemetry.gps.track;
				gps_telem.sog = telemetry.gps.sog;
				gps_telem.accuracy.quality = telemetry.gps.accuracy.quality;
				gps_telem.accuracy.sats = telemetry.gps.accuracy.sats;
				gps_telem.accuracy.pdop = telemetry.gps.accuracy.pdop;

				CMD_CMD((fifo_msg.data)) = CMD_GPS_TELEM;
				CMD_NUM((fifo_msg.data)) = sizeof(GPSTelem_t);
				CopyStructToBuf(gps_telem, dataPtr);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_TELEM,i)) != NULL)
				{
					Client * me = shmPtr->clients.getClient(0);
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}

				last_telem_tx.stamp();
			}
		}
	}

	currentStamp.stamp();
	if((currentStamp - last_position_update ) > (POSITION_UPDATE_PERIOD * SEC2MICRO) ) {
		last_position_update.stamp();
		moveUA();
	}
}

void APSimThread::moveUA()
{
	float psi_rate = 0;

	float psides = 0;
	float vdes = 0;
	Velocity_t vel;
	float psidotdes = 0;

	Client * a_client = shmPtr->clients.getClient("Tempest0");
	if(a_client != NULL) {
		if(a_client->telemetry.gps.pos.lat == 0) {
			subscribe(CMD_TELEM,a_client,true,INBOARD_TX);
			return;
		}
		float r = GPSRange( telemetry.gps.pos, a_client->telemetry.gps.pos);
		vdes = limits.v_max * (200000 / (r*r));
		printf("range: %f vdes: %f\n",r,vdes);
	} else {
		vdes = limits.v_max;
	}

	if(ap_mode == AP_FLYING && (ob_mode == OB_WAYPT || ob_mode == OB_OFF) ) {
		vectorField(telemetry.gps.pos, telemetry.gps.track, waypoints[current_waypoint].pos, 
				waypoints[current_waypoint].orbitRad, telemetry.gps.sog, &vel, &psides, &psidotdes);
		psi_rate = getTurnRate((float)telemetry.gps.track*M_PI/180, psides, psidotdes, 0.5);

	}
	else if(ap_mode == AP_FLYING && ob_mode == OB_TURNRATE ) {
		psi_rate = ap_rates.turn_rate;
		vdes = ap_rates.velocity;
	} 

	if( ap_mode == AP_FLYING ) {

		if(psi_rate > limits.dpsi_max) psi_rate = limits.dpsi_max;
		else if(psi_rate < -limits.dpsi_max) psi_rate = -limits.dpsi_max;

		if(vdes > limits.v_max)       vdes = limits.v_max;
		else if(vdes < limits.v_min)  vdes = limits.v_min;

		telemetry.gps.sog = vdes;

		//psi_rate *= TELEMETRY_PERIOD;

		srand(time(0));
    if(telemetry.gps.sog != 0.0) 
      //psi_rate += (5*M_PI/180.0)*((float)(rand())/(float)RAND_MAX - 0.5); 
      psi_rate += (M_PI/180.0)*((float)(rand())/(float)RAND_MAX - 0.5); 

		float v_x = cos(telemetry.gps.track*M_PI/180+psi_rate*POSITION_UPDATE_PERIOD)*telemetry.gps.sog*POSITION_UPDATE_PERIOD;
		float v_y = sin(telemetry.gps.track*M_PI/180+psi_rate*POSITION_UPDATE_PERIOD)*telemetry.gps.sog*POSITION_UPDATE_PERIOD;
		float v_z = (waypoints[current_waypoint].pos.alt - telemetry.gps.pos.alt) / (telemetry.gps.sog/4.0*POSITION_UPDATE_PERIOD); // FIXME -- better math needed here

    if(telemetry.gps.sog != 0.0) {
      v_x += 0.5*((float)(rand())/(float)RAND_MAX - 0.5) * POSITION_UPDATE_PERIOD;
      v_y += 0.5*((float)(rand())/(float)RAND_MAX - 0.5) * POSITION_UPDATE_PERIOD;
    }

		// convert units to degrees
		telemetry.gps.pos.lat += M_TO_LAT(v_x);
		telemetry.gps.pos.lon += M_TO_LON(v_y, telemetry.gps.pos.lat);
		telemetry.gps.pos.alt += v_z;
		if(telemetry.gps.sog != 0.0) 
			telemetry.gps.track    = atan2(v_y, v_x)*180.0/M_PI;
		//telemetry.angles.roll  = atan2(psi_rate*telemetry.gps.sog,9.8)*180.0/M_PI; 
		
		// convert units to milirad
		telemetry.angles.roll  = (int16_t)(atan2(psi_rate*telemetry.gps.sog,9.8)*1000.0); 
		telemetry.angles.yaw  = (int16_t)(telemetry.gps.track * M_PI/180 * 1000.0); 
	}

	while (telemetry.gps.track > 360)
		telemetry.gps.track = telemetry.gps.track-360;
	while (telemetry.gps.track < 0)
		telemetry.gps.track = telemetry.gps.track+360;

	//telemetry.gps.pos.alt = 1770;

	Client * me = shmPtr->clients.getClient(0);


	mutex_lock();
	me->telemetry.gps.pos.lat = telemetry.gps.pos.lat;
	me->telemetry.gps.pos.lon = telemetry.gps.pos.lon;
	me->telemetry.gps.pos.alt = telemetry.gps.pos.alt;
	me->telemetry.gps.track = telemetry.gps.track;
	me->telemetry.gps.sog = telemetry.gps.sog;
	me->telemetry.airdata.tas = telemetry.gps.sog;

	me->telemetry.navigation.APon   = 1;
	me->telemetry.navigation.MAmode = APMA_STANDARD;
	me->telemetry.navigation.APmode = ap_mode;
	me->telemetry.navigation.waypt  = current_waypoint;
	me->telemetry.navigation.OBmode = ob_mode;
	me->telemetry.navigation.OBtype = OB_NONE;
	me->telemetry.navigation.value  = current_waypoint;
	if( ob_mode == OB_TURNRATE )
		me->telemetry.navigation.value = (int8_t)Round(psi_rate*180.0/M_PI);
	mutex_unlock();

	float r = GPSRange( telemetry.gps.pos, waypoints[current_waypoint].pos);

	if(waypoints[current_waypoint].orbitRad == 0)
		if(r < (telemetry.gps.sog*10)) {
			current_waypoint = waypoints[current_waypoint].next;
		}
}

APSimThread::~APSimThread()
{
}

